#include <Servo.h>

#include <Usb.h>
#include <AndroidAccessory.h>

AndroidAccessory acc("Google, Inc.",
		"Skipper",
		"Servo Controller Board",
		"1.0",
		"http://goo.gl/skprbot",
		"0000000012345678");

const int N_SERVOS = 18;

Servo servos[N_SERVOS];

void stopCommand();
void attachServo(byte servoNum, byte pinNum);
void detachServo(byte servoNum);
void moveServo(byte servoNum, byte angle);
void batchMoveServos(byte *buffer);

void setup() {
	Serial.begin(115200);
	Serial.print("\r\nStart");

	acc.powerOn();
}

// detach all servos and stop all other activity
const byte STOP_COMMAND = 's';

const byte ATTACH_SERVO_COMMAND = 'a';
const byte ATTACH_SERVO_COMMAND_SIZE = 2;

const byte DETACH_SERVO_COMMAND = 'd';
const byte DETACH_SERVO_COMMAND_SIZE = 1;

const byte MOVE_SERVO_COMMAND = 'm';
const byte MOVE_SERVO_COMMAND_SIZE = 2;

const byte BATCH_MOVE_SERVOS_COMMAND = 'b';
const byte BATCH_MOVE_SERVOS_COMMAND_SIZE = N_SERVOS;

const byte MAX_MESSAGE_SIZE = 24;

void loop() {
	byte cmd;

	if (acc.isConnected()) {
		int len = acc.read(&cmd, sizeof(cmd), 1);
		byte buffer[MAX_MESSAGE_SIZE];

		if (len > 0) {
			Serial.print("Command: ");
			Serial.println(cmd);
			switch (cmd) {
			case STOP_COMMAND:
				// no bytes to read
			  stopCommand();
				break;
				
			case ATTACH_SERVO_COMMAND:
			  len = acc.read(buffer, ATTACH_SERVO_COMMAND_SIZE);
			  if (len == ATTACH_SERVO_COMMAND_SIZE) {
					attachServo(buffer[0], buffer[1]);
			  }
				break;
				
			case DETACH_SERVO_COMMAND:
			  len = acc.read(buffer, DETACH_SERVO_COMMAND_SIZE);
			  if (len == DETACH_SERVO_COMMAND_SIZE) {
					detachServo(buffer[0]);
			  }
				break;
				
			case MOVE_SERVO_COMMAND:
			  len = acc.read(buffer, MOVE_SERVO_COMMAND_SIZE);
			  if (len == MOVE_SERVO_COMMAND_SIZE) {
					moveServo(buffer[0], buffer[1]);
			  }
				break;
				
			case BATCH_MOVE_SERVOS_COMMAND:
			  len = acc.read(buffer, BATCH_MOVE_SERVOS_COMMAND_SIZE);
			  if (len == BATCH_MOVE_SERVOS_COMMAND_SIZE) {
					batchMoveServos(buffer);
			  }
				break;				
			}
		}

//		msg[0] = 0x1;
//		acc.write(msg, 3);

		// b = digitalRead(BUTTON1);
		// if (b != b1) {
		// 	msg[1] = 0;
		// 	msg[2] = b ? 0 : 1;
		// 	acc.write(msg, 3);
		// 	b1 = b;
		// }
	} else {
		// reset outputs to default values on disconnect
		stopCommand();
	}

	delay(10);
}

void attachServo(byte servoNum, byte pinNum) {
	Serial.print("Attaching ");
	Serial.print(servoNum, DEC);
	Serial.print(" to ");
	Serial.println(pinNum, DEC);
	
	servos[servoNum].attach(pinNum);
}

void detachServo(byte servoNum) {
  Serial.print("Detaching ");
	Serial.print(servoNum, DEC);
	servos[servoNum].detach();
}

void stopCommand() {
	for (int i = 0; i < N_SERVOS; i++) {
		if (servos[i].attached()) {
			servos[i].detach();
		}
	}
}

void moveServo(byte servoNum, byte angle) {
	Serial.print("Moving ");
	Serial.print(servoNum, DEC);
	Serial.print(" to ");
	Serial.println(angle, DEC);
	if (servos[servoNum].attached()) {
		if (angle <= 180) {
			servos[servoNum].write(angle);
		}
	} else {
		Serial.println("Not attached");
	}	
}

void batchMoveServos(byte *buffer) {
	for (int i = 0; i < N_SERVOS; i++) {
		if (servos[i].attached()) {
			Serial.print("Moving ");
			Serial.print(i, DEC);
			Serial.print(" to ");
			Serial.println(buffer[i], DEC);
			servos[i].write(buffer[i]);
		}
	}
}
